Dr. Joshua Vaughan
joshua.vaughan@louisiana.edu
http://www.ucs.louisiana.edu/~jev9637/
In this example, we'll determine the equations of motion for the simple pendulum system shown in Figure 1. The system consists of a point mass, $m$, connected to an ideal pin by a massless rod of length $l$. The rotation of the pendulum is represented by $\theta$.
Figure 1: Simple Pendulum
# Import the SymPy Module
import sympy
# Import the necessary sub-modules and methods for dynamics
from sympy.physics.mechanics import dynamicsymbols
from sympy.physics.mechanics import LagrangesMethod
from sympy.physics.mechanics import Particle, Point, ReferenceFrame
# initiate better printing of SymPy results
sympy.init_printing()
# Define the genearlized coordinate
theta = dynamicsymbols('theta')
# Also define the first derivative
theta_dot = dynamicsymbols('theta', 1)
# Define the symbols for the other paramters
m, g, l, t = sympy.symbols('m, g, l, t')
# Define the Newtonian reference frame
N = ReferenceFrame('N')
# Define a body-fixed frame along the pendulum, with y aligned from m to the pin
A = N.orientnew('A', 'Axis', [theta, N.z])
# Define the points and set its velocity
P = Point('P')
P.set_vel(N, l * theta_dot * A.x)
mp = Particle('mp', P, m)
# Set up the force list - each item follows the form:
# (the location where the force is applied, its magnitude and direction)
# Here, there are no non-conservataive external forces
forces = []
# Form the Lagrangian - L = T - V
L = (1 / 2 * m * l**2 * theta_dot**2) + (m * g * l * sympy.cos(theta))
# This creates a LagrangesMethod class instance that will allow us to form the equations of motion, etc
LM = LagrangesMethod(L, [theta], forcelist = forces, frame = N)
LM.form_lagranges_equations()
The LagrangesMethod class gives us lots of information about the system. For example, we can output the mass/inertia matrix and the forcing terms. Note that the forcing terms include what might be conservative forces and would therefore normally appear in a stiffness matrix.
# Output the inertia/mass matrix of the system
LM.mass_matrix
# Output the forcing terms of the system
LM.forcing
We can also use builtin functions to write the sytsem as a set of first order ODEs, suitable for simluation.
# Make the call to set up in state-space-ish form q_dot = f(q, t)
lrhs = LM.rhs()
# Simplify the results
lrhs.simplify()
# Output the result
lrhs
We can also linearize these equations with builtin SymPy methods. Let's do so about the $\theta = 0$, $\dot{\theta} = 0$ operating point. The resulting equations returned are a system of first order ODEs in state-space form:
$$ \dot{x} = Ax + Bu $$See the SymPy Documentation for much more information.
# Define the point to linearize around
operating_point = {theta: 0.0, theta_dot: 0.0}
# Make the call to the linearizer
A, B, inp_vec = LM.linearize([theta], [theta_dot],
op_point = operating_point,
A_and_B = True)
A
B
The $B$ matrix is empty, as we expect, since there are have no external non-conservative forces.
Given these two matrices, the system of equations match those that we'd expect:
$$ \begin{bmatrix}\dot{w}_1 \\ \dot{w}_2\end{bmatrix} = \begin{bmatrix}0 & 1 \\ -\frac{g}{l} & 0 \end{bmatrix} \begin{bmatrix}w_1 \\ w_2\end{bmatrix}$$where:
$$ \mathbf{w} = \begin{bmatrix}w_1 \\ w_2\end{bmatrix} = \begin{bmatrix}\theta \\ \dot{\theta}\end{bmatrix} $$We can pass these equations of motion to numerical solver for simluation. To do so, we need to import NumPy and the SciPy ode solver, ode
. We'll also import matplotlib to enable plotting of the results.
For a system as simple as this one, we could easily set up the necessary components for the numerical simulation manually. However, here will automate as much as possible. Following a similar procedure on more complicated systems would be necessary.
# import NumPy with namespace np
import numpy as np
# import the ode ODE solver
from scipy.integrate import ode
# import the plotting functions from matplotlib
import matplotlib.pyplot as plt
# set up the notebook to display the plots inline
%matplotlib inline
# Define the states and state vector
w1, w2 = sympy.symbols('w1 w2', cls=sympy.Function)
w = [w1(t), w2(t)]
# Set up the state definitions and parameter substitution
sub_params = {theta: w1(t),
theta_dot: w2(t),
g : 9.81,
l : 2.0}
# Create a function from the equations of motion
# Here, we substitude the states and parameters as appropriate prior to the lamdification
eq_of_motion = sympy.lambdify((t, w),
lrhs.subs(sub_params))
# Set up the initial conditions for the solver
theta_init = 10 * np.pi/180 # Initial angle
theta_dot_init = 0.0 # Initial angular velocity
# Pack the initial conditions into an array
x0 = [theta_init, theta_dot_init]
# Create the time samples for the output of the ODE solver
sim_time = np.linspace(0.0, 10.0, 1001) # 0-10s with 1001 points in between
# Set up the initial point for the ode solver
r = ode(eq_of_motion).set_initial_value(x0, sim_time[0])
# define the sample time
dt = sim_time[1] - sim_time[0]
# pre-populate the response array with zeros
response = np.zeros((len(sim_time), len(x0)))
# Set the initial index to 0
index = 0
# Now, numerically integrate the ODE while:
# 1. the last step was successful
# 2. the current time is less than the desired simluation end time
while r.successful() and r.t < sim_time[-1]:
response[index, :] = r.y
r.integrate(r.t + dt)
index += 1
Now, let's plot the results. The first column of the response
vector is the angle of the pendulum, $\theta$. We plot that below, after setting up plotting parameters to make the plot more readable.
# Set the plot size - 3x2 aspect ratio is best
fig = plt.figure(figsize=(6, 4))
ax = plt.gca()
plt.subplots_adjust(bottom=0.17, left=0.17, top=0.96, right=0.96)
# Change the axis units to serif
plt.setp(ax.get_ymajorticklabels(), family='serif', fontsize=18)
plt.setp(ax.get_xmajorticklabels(), family='serif', fontsize=18)
# Remove top and right axes border
ax.spines['right'].set_color('none')
ax.spines['top'].set_color('none')
# Only show axes ticks on the bottom and left axes
ax.xaxis.set_ticks_position('bottom')
ax.yaxis.set_ticks_position('left')
# Turn on the plot grid and set appropriate linestyle and color
ax.grid(True,linestyle=':', color='0.75')
ax.set_axisbelow(True)
# Define the X and Y axis labels
plt.xlabel('Time (s)', family='serif', fontsize=22, weight='bold', labelpad=5)
plt.ylabel('Angle (deg)', family='serif', fontsize=22, weight='bold', labelpad=10)
# Plot the data
plt.plot(sim_time, response[:, 0] * 180/np.pi, linewidth=2, linestyle='-', label = '$\theta$')
# uncomment below and set limits if needed
# plt.xlim(0, 5)
# plt.ylim(-1, 1)
# Create the legend, then fix the fontsize
# leg = plt.legend(loc='upper right', ncol = 1, fancybox=True)
# ltext = leg.get_texts()
# plt.setp(ltext, family='serif', fontsize=20)
# Adjust the page layout filling the page using the new tight_layout command
plt.tight_layout(pad=0.5)
# Uncomment to save the figure as a high-res pdf in the current folder
# It's saved at the original 6x4 size
# plt.savefig('Simple_Pendulum_Response.pdf')
fig.set_size_inches(9, 6) # Resize the figure for better display in the notebook
Code is licensed under a 3-clause BSD style license. See the licenses/LICENSE.md file.
Other content is provided under a Creative Commons Attribution-NonCommercial 4.0 International License, CC-BY-NC 4.0.
# This cell will just improve the styling of the notebook
# You can ignore it, if you are okay with the default sytling
from IPython.core.display import HTML
import urllib.request
response = urllib.request.urlopen("https://cl.ly/1B1y452Z1d35")
HTML(response.read().decode("utf-8"))